#ifndef TRUNK_NODE
#define TRUNK_NODE

#include <ros/ros.h>
#include <threemxl/C3mxlROS.h>
#include <XMLConfiguration.h>
#include <threemxl/dxlassert.h>
#include "topic/topic.h"
#include "comm/trunk_msg.h"
#include "config.h"

#define TRUNK_POS_NEUTRAL	0.8
#define TRUNK_POS_UP		0
#define TRUNK_POS_DOWN		2.5
#define TRUNK_SPEED			0.8

class TrunkNode
{
private:
	ros::NodeHandle nh_;
	ros::Subscriber motor_sub_;
	ros::Publisher status_pub_;

	LxSerial serial_port_;	// serial port interface
	C3mxlROS *motor_;

	int trunk_cstate, trunk_fstate;
	ros::Time timestamp_;
public:
	TrunkNode();
	~TrunkNode();

	/**
	 * Moves the motors to the start position.
	 */
	void extInit();

	/**
	 * Handle incoming messages for the trunk.
	 * @param	msg	expected state for the trunk.
	 */
	void trunkCallback(const comm::trunk::ConstPtr &msg);

	/**
	 * Initialize motor parameters.
	 */
	void init();

	/**
	 * Spin node.
	 */
	void spin();
};


#endif
